#include "dynamixel.h"
#include "cm5.h"
#include "usart1.h"

uint8_t cm5_controlTable[CM5_MAX_ADDRESS];

static struct REG_WRITE_BUFFER
{
    uint8_t numBytes;
    uint8_t startAddress;
    uint8_t buffer[CM5_MAX_ADDRESS];
} regWriteBuffer;


// initialize the control table
void initCM5()
{
    uint8_t i;
    for(i=0; i<CM5_MAX_ADDRESS; i++)
    {
        cm5_controlTable[i] = 0x00;
    }

    // set specific values
    cm5_controlTable[CM5_RETURN_LEVEL] = DXL_RETURN_LEVEL_READ_ONLY; // respond only to READ commands
}


static void writeData(DXL_Packet *packet)
{
    uint8_t startAddress = (packet->params)[0];
    uint8_t i;
    for(i=1; i<packet->paramLength; i++)
    {
        if(startAddress+i < CM5_MAX_ADDRESS)
        {
            cm5_controlTable[i+startAddress] = (packet->params)[i];

            // TODO: change h/w registers for certain control table settings
            // e.g. baud rate, LEDs
        }

    }
}

static void regWrite(DXL_Packet *packet)
{
    // buffer changes for a future action command
    uint8_t startAddress = (packet->params)[0];
    uint8_t i;
    for(i=1; i<packet->paramLength; i++)
    {
        if(startAddress+i < CM5_MAX_ADDRESS)
        {
            regWriteBuffer.buffer[i+startAddress] = (packet->params)[i];
        }
    }
    regWriteBuffer.startAddress = startAddress;
    regWriteBuffer.numBytes = packet->paramLength;
}

static void action(DXL_Packet *packet)
{
    (void)packet;
    uint8_t i;

    for(i=0; i<regWriteBuffer.numBytes; i++)
    {
        cm5_controlTable[regWriteBuffer.startAddress + i] = regWriteBuffer.buffer[regWriteBuffer.startAddress + i];
    }
}

static void readData(DXL_Packet *packet, DXL_Packet *reply)
{
    uint8_t startAddress = (packet->params)[0];
    uint8_t numBytes = (packet->params)[1];
    uint8_t i;

    for(i=0; i<numBytes; i++)
    {
        if(i+startAddress < CM5_MAX_ADDRESS)
        {
            (reply->params)[i] = cm5_controlTable[i+startAddress];
        }
    }
    reply->paramLength = numBytes;
}

static void syncWrite(DXL_Packet *packet)
{
    // only process the portion of the packet intended for us
    // the listener thread will pass the packet on to the other dynamixels
    uint8_t startAddress = (packet->params)[0];
    uint8_t numBytes = (packet->params)[1];
    uint8_t length = packet->paramLength;
    uint8_t i;

    for(i=2; i<length && (packet->params)[i] != CONTROLLER_ID; i+= numBytes);

    if((packet->params)[i] == CONTROLLER_ID)
    {
        // there is some data to write locally
        uint8_t j;
        for(j=0; j<numBytes; i++)
        {
            cm5_controlTable[startAddress+j] = (packet->params)[i+j];
        }
    }
}


void processPacket(DXL_Packet *packet)
{
    BOOL sendReply = cm5_controlTable[CM5_RETURN_LEVEL] == DXL_RETURN_LEVEL_ALWAYS;
    DXL_Packet response;
    response.id = CONTROLLER_ID;
    response.instrErr = 0;
    response.paramLength = 0;

    switch(packet->id)
    {
    case DXL_WRITE_DATA:
        writeData(packet);
        break;

    case DXL_READ_DATA:
        readData(packet, &response);

        // only send the response if we are configured to send one
        if(cm5_controlTable[CM5_RETURN_LEVEL] != DXL_RETURN_LEVEL_NEVER)
            sendReply = TRUE;
        break;

    case DXL_PING:
        // just create and send a status packet (regardless of the return level)
        sendReply = TRUE;
        break;

    case DXL_ACTION:
        action(packet);
        break;

    case DXL_RESET:
        // reset the CM5 control table to default
        initCM5();
        break;

    case DXL_REG_WRITE:
        regWrite(packet);
        break;

    case DXL_SYNC_WRITE:
        syncWrite(packet);
        break;

    default:
        // unknown command
        sendReply = TRUE;
        response.instrErr = DXL_INSTRUCTION_ERROR;
        break;
    }

    if(sendReply)
    {
        createChecksum(&response);
        transmitPacketUSART1(&response);
    }
}
